import numpy as np
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
import math
import para
cap = cv2.VideoCapture(0,cv2.CAP_MSMF)#打开内置摄像机
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,3840)


w = 6
h = 4
def corner(img):#找角点
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    ret, corners = cv2.findChessboardCorners(gray, (w,h),None)
    if ret == True:
        cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
    return corners
'''
L=camera(IMAX=[[1.56946348e+03,0.00000000e+00,9.65258096e+02]#imgL_point_424
             ,[0.00000000e+00,1.56793866e+03,4.95980421e+02]
             ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-7.30986552e-02 ,3.99374120e-01 ,9.28169866e-04 ,2.82906248e-04 ,-9.73325096e-01])
'''
L=camera(IMAX=[[1.57366985e+03 ,0.00000000e+00 ,9.42156018e+02]#imgL_cross_424
 ,[0.00000000e+00 ,1.56435543e+03 ,5.14905625e+02]
 ,[0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]],
         DIST=[-0.03020629 ,-0.01574204 , 0.00609551 ,-0.00025611  ,0.10228617])
'''
R=camera(IMAX=[[1.57018321e+03,0.00000000e+00,9.56921749e+02]#imgR_point_424
             ,[0.00000000e+00,1.56994540e+03,4.95969393e+02]
             ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-0.07213587 ,0.29454981,-0.00159414 ,0.00068262,-0.35742506])
'''
R=camera(IMAX= [[1.57534870e+03,0.00000000e+00,9.12358094e+02]
 ,[0.00000000e+00,1.57159939e+03,5.11227097e+02]
 ,[0.00000000e+00,0.00000000e+00,1.00000000e+00]],
         DIST=[-0.0651569   ,0.36046137 , 0.00704255 ,-0.00412844 ,-0.88801585])
while cap.isOpened():#当摄像头打开时
    try:
        ret,frame=cap.read()#读取当前摄像头画面
        l=frame[:,0:1920]
        r=frame[:,1920:3840]
        l=L.undistort(l)#还是不去畸变吧
        r=R.undistort(r)
        #cv2.imshow('r',r)
        #cv2.waitKey(1)
        l_corners=corner(l)
        cv2.drawChessboardCorners(l, (w,h), l_corners, True)
        r_corners=corner(r)
        
        cv2.drawChessboardCorners(r, (w,h), r_corners, True)
        pos=[]
        for i in range(24):
            a=(l_corners[i,0,0]-L.cx,l_corners[i,0,1]-L.cy)#转换为图像坐标系
            b=(r_corners[i,0,0]-R.cx,r_corners[i,0,1]-R.cy)
            print("y:",a[0]-b[0])
            z=120/(a[0]/L.fx-b[0]/R.fx)#基线长度120mm,摄像机坐标系原点在左侧相机的镜头(小孔成像模型中的小孔)
            x=z*a[0]/L.fx
            y=z*a[1]/L.fy

            pos.append((x,y,z))
            x=round(x,1)
            y=round(y,1)
            z=round(z,1)
            #cv2.putText(l,str(x)+' '+str(y)+' '+str(z),(int(l_corners[i,0,0]),int(l_corners[i,0,1])),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)#把在摄像机坐标系中的坐标标注在图上

            cv2.putText(l,str(round(a[1]-b[1],1)),(int(l_corners[i,0,0]),int(l_corners[i,0,1])),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1)#把在摄像机坐标系中的坐标标注在图上
        #print(math.sqrt((pos[0][0]-pos[-1][0])**2+(pos[0][1]-pos[-1][1])**2+(pos[0][2]-pos[-1][2])**2))
        print("dis:",math.sqrt((pos[0][0]-pos[5][0])**2+(pos[0][1]-pos[5][1])**2+(pos[0][2]-pos[5][2])**2))
        #print(pos[0][0]-pos[5][0],pos[0][1]-pos[5][1],pos[0][2]-pos[5][2])
        cv2.imshow('l',l)
        if cv2.waitKey(1)==ord(' '):
            cv2.imwrite("L.png",l)
    except:#没识别到全部角点(
        pass    